#include "vbb_Utils.h"
#include "vbb_SENSOR.h"

/**
* These functions are designed to live in RAM. In the case the device
* fails to check-in every 2 minutes, it will automatically reboot.
*/

#define WATCHDOG_COUNTDOWN 1024

char _utils_deviceModel;
char _utils_deviceDetected;
char _utils_pinOn;
char _utils_FlashUnlocked;
unsigned int _utils_watchCountdown;
char _utils_IsDebugging;

VBB_Pin ledRed;
VBB_Pin ledGreen;
VBB_Pin buzzPin;

#define SAVE_HEADER                     0
#define SAVE_RTC                        1
#define SAVE_RTC_TIMEOFFSET             2

bool _utils_output_init;

void vbb_EssentialInit(bool DebugMode)
{
    _utils_output_init = FALSE;
    vbb_DelayInit();
    vbb_WatchDog_Init(vbb_DeviceModel());
    
    if (vbb_DeviceModel() == CNS03)
    {
        vbb_PIN_Init(&ledRed, GPIOE, 15, PIN_MODE_GENERAL_OUTPUT_PUSH_PULL);
        vbb_PIN_Init(&ledGreen, GPIOE, 14, PIN_MODE_GENERAL_OUTPUT_PUSH_PULL);

        vbb_PIN_Init(&buzzPin, GPIOD, 10, PIN_MODE_GENERAL_OUTPUT_PUSH_PULL);
    }
    else if (vbb_DeviceModel() == CNS01 || vbb_DeviceModel() == BA01)
    {
        vbb_PIN_Init(&ledRed, GPIOE, 8, PIN_MODE_GENERAL_OUTPUT_PUSH_PULL);
        vbb_PIN_Init(&ledGreen, GPIOE, 10, PIN_MODE_GENERAL_OUTPUT_PUSH_PULL);

        vbb_PIN_Init(&buzzPin, GPIOB, 12, PIN_MODE_GENERAL_OUTPUT_PUSH_PULL);
    }    
    
    vbb_OUTPUT_Init();
    vbb_DelayMillis(100);
    vbb_OUTPUT_Print(".\r\n");
    vbb_OUTPUT_Print(".\r\n");
    vbb_OUTPUT_Print(".\r\n");
    vbb_OUTPUT_Print("INFO: Essential firmware functionalities initialized.\r\n");
    if (vbb_DeviceModel() == CNS01)
        vbb_OUTPUT_Print("INFO: Device model is CNS1.\r\n");    
    else if (vbb_DeviceModel() == CNS03)
        vbb_OUTPUT_Print("INFO: Device model is CNS3.\r\n");  
    else if (vbb_DeviceModel() == BA01)
			vbb_OUTPUT_Print("INFO: Device model is BA01.\r\n"); 			
    
    vbb_IndependentWatchDogInit();
    
    _utils_IsDebugging = DebugMode;
}

bool vbb_DebugMode(void)
{
    return _utils_IsDebugging;
}

void vbb_SetDebugMode(bool DebugMode)
{
    _utils_IsDebugging = DebugMode;
}

void vbb_DeviceDetect(void)
{    
    RCC->APB2ENR |= RCC_APB2ENR_IOPEEN;
    GPIOE->CRH &= ~(PIN_MODE_CLEAR << ((9 - 8) * 4));
    GPIOE->CRH |= (PIN_MODE_FLOATING_INPUT << ((9 - 8) * 4));        
    
    if (GPIOE->IDR & (1UL << 9))
        _utils_deviceModel = CNS03;
    else
    {
        GPIOE->CRL &= ~(PIN_MODE_CLEAR << (2 * 4));
        GPIOE->CRL |= (PIN_MODE_FLOATING_INPUT << (2 * 4)); 
        
        if (GPIOE->IDR & (1UL << 2))        
            _utils_deviceModel = BA01;
        else
            _utils_deviceModel = CNS01;
        
        GPIOE->CRL &= ~(PIN_MODE_CLEAR << (2 * 4));
    }
    
    GPIOE->CRH &= ~(PIN_MODE_CLEAR << ((9 - 8) * 4));
    _utils_deviceDetected = 1;
}

char vbb_DeviceModel(void)
{
    if (!_utils_deviceDetected)
        vbb_DeviceDetect();
    return _utils_deviceModel;
}

/**
* Init the delay functionality with the resolution at milliseconds 
* and microseconds.
*/
char _utils_DelayInit = FALSE;
void vbb_DelayInit(void)
{    
    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
    TIM2->ARR = 0xFFFF;
    TIM2->PSC = 36000 - 1;
    TIM2->CR1 = 0;
    TIM2->CR2 = 0;
    TIM2->CR1 |= TIM_CR1_CEN;
    DBGMCU->CR = DBGMCU_CR_DBG_TIM2_STOP;
    _utils_DelayInit = TRUE;
}

void vbb_DelayMillis(unsigned short Milliseconds)
{        
    unsigned int _count;
    
    if (_utils_DelayInit)
    {
        TIM2->ARR = Milliseconds;
        TIM2->CNT = 0;
        while (!(TIM2->SR & TIM_SR_UIF))
        {
        }   
        TIM2->SR &= ~TIM_SR_UIF;
        
        TIM2->ARR = Milliseconds;
        TIM2->CNT = 0;
        while (!(TIM2->SR & TIM_SR_UIF))
        {
        }   
        TIM2->SR &= ~TIM_SR_UIF;
    }
    else
    {
        _count = Milliseconds * 72000;
        while (_count > 0)
            _count--;
    }
}

void vbb_Reset(void)
{     
    vbb_DelayMillis(1000);
    SCB->AIRCR  = ((0x5FA << 16) | (SCB->AIRCR & (0x700)) | (1 << 2));
    __DSB();                                                    
    while (1);
}

bool _utils_pulse_simulate;

extern bool booted;

void SoftwareWatchdogUpdate(void)
{
    if (!_utils_pinOn)
    {
        if (_utils_deviceModel == CNS03)
            GPIOD->BSRR = 1 << 11;                    
        _utils_pinOn = 1;       
#ifndef __TINY_TEST__
        if (_utils_pulse_simulate)
            vbb_SENSOR_PulseFire();                
#endif        
        _utils_watchCountdown--;                
        vbb_WatchDog_CheckIn();
        
        if (_utils_watchCountdown <= 0)
        {
            vbb_OUTPUT_Print("INFO: Reboot because of software watchdog.\r\n");
            vbb_Reset();
        }
    }
    else
    {
        if (_utils_deviceModel == CNS03)
            GPIOD->BSRR = 1 << (11 + 16);
        _utils_pinOn = 0;
    }    
}

void TIM4_IRQHandler(void)
{	
	if (TIM4->SR & TIM_SR_UIF)
    {        
        SoftwareWatchdogUpdate();        
        TIM4->SR &= ~(TIM_SR_UIF);
    }
} 

void vbb_WatchDog_CheckIn(void)
{
    //vbb_OUTPUT_Print("INFO: Software watchdog checkin.\r\n");
    _utils_watchCountdown = WATCHDOG_COUNTDOWN;
    //vbb_OUTPUT_Print("[!]");
    IWDG->KR  = 0xAAAA;
}

void vbb_WatchDog_Init(char DeviceModel)
{
    _utils_deviceModel = DeviceModel;        
    _utils_watchCountdown = WATCHDOG_COUNTDOWN;
    
    RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
    
    _utils_pinOn = 0;
    
    if (DeviceModel == CNS03)
    {
        RCC->APB2ENR |= RCC_APB2ENR_IOPDEN;
        GPIOD->CRH &= ~(0x0FUL << 12);
        GPIOD->CRH |= (0x03UL << 12);
    }
    
    // 2013.09.26 Changed to much faster rate
    TIM4->ARR = 0xFFF;
    TIM4->PSC = 720 - 1;
    
    TIM4->CR1 = 0;
    TIM4->CR2 = 0;
    
    TIM4->DIER = 1;
    
    NVIC_SetPriority(TIM4_IRQn, 5);
    NVIC_EnableIRQ(TIM4_IRQn);
    
    TIM4->CR1 |= TIM_CR1_CEN;     
}

void vbb_BACKUP_Set(unsigned int RegId, unsigned short Data)
{
    if (vbb_BACKUP_Get(RegId) == Data)
        return;
    
    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
    RCC->APB1ENR |= RCC_APB1ENR_BKPEN;
    PWR->CR |= PWR_CR_DBP;
    vbb_DelayMillis(10);
    
    *((unsigned short *)(&(BKP->DR1) + RegId * sizeof(unsigned short))) = Data;    
    vbb_DelayMillis(10);
}

void vbb_BACKUP_SetInt(unsigned int RegId, unsigned int Data)
{
    vbb_BACKUP_Set(RegId, (Data & 0xFFFF0000) >> 16);
    vbb_BACKUP_Set(RegId + 1, (Data & 0x0000FFFF));
}

unsigned short vbb_BACKUP_Get(unsigned int RegId)
{
    return *((unsigned short *)(&(BKP->DR1) + RegId * sizeof(unsigned short)));
}

unsigned int vbb_BACKUP_GetInt(unsigned int RegId)
{
    unsigned int retValue = 0;
    retValue |= ((vbb_BACKUP_Get(RegId) & 0x0000FFFF) << 16);    
    retValue |= (vbb_BACKUP_Get(RegId + 1) & 0x0000FFFF);
    return retValue;
}

void vbb_BACKUP_Clear(void)
{
    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
    RCC->APB1ENR |= RCC_APB1ENR_BKPEN;
    PWR->CR |= PWR_CR_DBP;    
    RCC->BDCR = RCC_BDCR_BDRST; // Reset Backup Domain	
    vbb_DelayMillis(10);    
    RCC->BDCR = 0;
}

#define FLASH_PAGE_SIZE 0x800

void INFLASH_Unlock(void)
{
    if (!_utils_FlashUnlocked)
    {
        FLASH->CR &= ~FLASH_CR_EOPIE;
        FLASH->CR &= ~FLASH_CR_ERRIE;
        FLASH->CR |= FLASH_CR_OPTWRE;
        FLASH->CR |= FLASH_CR_OPTER;
        FLASH->CR |= FLASH_CR_OPTPG;
        
        FLASH->SR = FLASH_SR_BSY | FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPRTERR;
           
        FLASH->KEYR = 0x45670123;
        FLASH->KEYR = 0xCDEF89AB;
        
        FLASH->SR = FLASH_SR_BSY | FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPRTERR;
        
        _utils_FlashUnlocked = 1;
    }
}

void vbb_INFLASH_ErasePage(unsigned int Address)
{
    INFLASH_Unlock();
    
    while (READ_BIT(FLASH->SR, FLASH_SR_BSY))
    {
    }
    
    FLASH->CR |= FLASH_CR_PER;
    FLASH->AR = Address;
    FLASH->CR |= FLASH_CR_STRT;
    
    while (READ_BIT(FLASH->SR, FLASH_SR_BSY))
    {
    }
    
    FLASH->CR &= ~FLASH_CR_PER;
}

unsigned short vbb_INFLASH_ReadShort(unsigned int Address)
{
    return *((unsigned short *) Address);
}

char vbb_INFLASH_ReadByte(unsigned int Address)
{
    char ret;
    unsigned int RealAddr;
    short aShort;
    
    if ((Address & 1) == 0)
        RealAddr = Address;
    else
        RealAddr = Address - 1;
    
    aShort = *((unsigned short *) RealAddr);
    
    if ((Address & 1) == 0)
        ret = ((aShort & 0xFF00) >> 8);
    else
        ret = (aShort & 0x00FF);
    
    return ret;
}

unsigned int vbb_INFLASH_Program(unsigned int Address, unsigned short Data)
{    
    INFLASH_Unlock();        
    
    while (READ_BIT(FLASH->SR, FLASH_SR_BSY)) 
    {
    }
    
    FLASH->CR |= FLASH_CR_PG;
    *(__IO unsigned short *) Address = Data;
    
    
    while (READ_BIT(FLASH->SR, FLASH_SR_BSY)) 
    {
    }
    
    FLASH->CR &= ~FLASH_CR_PG;
    
    // Verify what we just programmed
    if ((*(__IO unsigned short *) Address) == Data)
        return Address += 2;
    else
        return 0; // Failed of programming verification
}

void vbb_OUTPUT_Init(void)
{
    RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    GPIOA->CRH &= ~(0x0FUL << ((9 - 8) * 4));
    GPIOA->CRH |= (0x0BUL << ((9 - 8) * 4));    
    GPIOA->CRH &= ~(0x0FUL << ((10 - 8) * 4));
    GPIOA->CRH |= (0x04UL << ((10 - 8) * 4));
    
    RCC->APB2ENR |=  RCC_APB2ENR_USART1EN;
    USART1->CR1 = USART_CR1_TE;
    USART1->CR2 &= ~(USART_CR2_STOP);
    USART1->CR3 = 0x0000;
    USART1->BRR = 72000000 / 9600;        
    vbb_DelayMillis(100);
    USART1->CR1 |= USART_CR1_UE;
    _utils_output_init = TRUE;
}

void vbb_OUTPUT_Deinit(void)
{   
    USART1->CR1 &= ~USART_CR1_UE;
    RCC->APB2ENR &= ~RCC_APB2ENR_USART1EN;      
}

void vbb_OUTPUT_WriteByte(char Byte)
{    
    if (!_utils_output_init) return;
    
    while (!READ_BIT(USART1->SR, USART_SR_TXE))
    {
    }    
    
    CLEAR_BIT(USART1->SR, USART_SR_TXE);
    USART1->DR = Byte;
    
    while (!READ_BIT(USART1->SR, USART_SR_TXE))
    {
    }
}

void vbb_OUTPUT_Print(const char * Text)
{    
    while (Text[0])
    {
        vbb_OUTPUT_WriteByte(Text[0]);
        Text++;
    }
}

void vbb_OUTPUT_PrintDebug(const char * Text)
{
    if (Text == NULL)
    {
        vbb_OUTPUT_WriteByte('N');
        vbb_OUTPUT_WriteByte('U');
        vbb_OUTPUT_WriteByte('L');
        vbb_OUTPUT_WriteByte('L');
        return;
    }    
    
    while (Text[0])
    {
        if (Text[0] == '\r')
        {
            vbb_OUTPUT_WriteByte('<');
            vbb_OUTPUT_WriteByte('C');
            vbb_OUTPUT_WriteByte('R');
            vbb_OUTPUT_WriteByte('>');
        }
        else if (Text[0] == '\n')
        {
            vbb_OUTPUT_WriteByte('<');
            vbb_OUTPUT_WriteByte('L');
            vbb_OUTPUT_WriteByte('F');
            vbb_OUTPUT_WriteByte('>');
        }
        else
            vbb_OUTPUT_WriteByte(Text[0]);
        Text++;
    }
}

void vbb_OUTPUT_PrintNumberUnSigned(unsigned int Number)
{
    unsigned int Div = 10;
    
    while (Div < Number)
        Div *= 10;
    
    if (Div > Number)
        Div /= 10;
    
    while (Div >= 10)
    {
        vbb_OUTPUT_WriteByte('0' + (Number / Div));
        Number = Number % Div;
        Div /= 10;
    }
    
    vbb_OUTPUT_WriteByte('0' + (Number % 10));
}

void vbb_OUTPUT_PrintNumberSigned(int Number)
{    
    if (Number < 0)
    {
        vbb_OUTPUT_WriteByte('-');
        Number = -Number;
    }
    
    vbb_OUTPUT_PrintNumberUnSigned(Number);
}

void vbb_PIN_Init(VBB_Pin * Pin, GPIO_TypeDef *  GPIOx, uint32_t P, uint32_t Mode)
{
    if (Mode == PIN_MODE_ALTERNATE_OUTPUT_PUSH_PULL)
        RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;
    
    Pin->GPIOx = GPIOx;
    Pin->p = P;    
    
    if (Pin->GPIOx == GPIOA)
        RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    else if (Pin->GPIOx == GPIOB)
        RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
    else if (Pin->GPIOx == GPIOC)
        RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;
    else if (Pin->GPIOx == GPIOD)
        RCC->APB2ENR |= RCC_APB2ENR_IOPDEN;
    else if (Pin->GPIOx == GPIOE)
        RCC->APB2ENR |= RCC_APB2ENR_IOPEEN;
    
    if (Pin->p < 8)
    {
        Pin->GPIOx->CRL &= ~(0x0FUL << (Pin->p * 4));
        Pin->GPIOx->CRL |= (Mode << (Pin->p * 4));
    }
    else
    {
        Pin->GPIOx->CRH &= ~(0x0FUL << ((Pin->p - 8) * 4));
        Pin->GPIOx->CRH |= (Mode << ((Pin->p - 8) * 4));
    }
    
    Pin->mode = Mode;
}

void vbb_PIN_Set(VBB_Pin * Pin, bool On)
{
    if (On)
    {
        Pin->GPIOx->BSRR = 1 << Pin->p;
        Pin->on = On;
    }
    else 
    {
        Pin->GPIOx->BSRR = 1 << (Pin->p + 16);
        Pin->on = On;
    }
}

bool vbb_PIN_Get(VBB_Pin * Pin)
{
    return (Pin->GPIOx->IDR & (1UL << Pin->p)) != 0;
}

void vbb_LedSwitch(char Red, char Green)
{    
    vbb_PIN_Set(&ledRed, Red);
    vbb_PIN_Set(&ledGreen, Green);
}

void vbb_Buzz(unsigned int MilliCounter)
{
    vbb_PIN_Set(&buzzPin, TRUE);
    vbb_DelayMillis(MilliCounter);
    vbb_PIN_Set(&buzzPin, FALSE);
    vbb_DelayMillis(MilliCounter);
}

#define __IWDG_PERIOD   0x001E8480
#define __IWDG_PR       6
#define __IWDGCLOCK     32000UL/256

#define __IWGDCLK  (32000UL/(0x04<<__IWDG_PR))
#define __IWDG_RLR (__IWDG_PERIOD*__IWGDCLK/1000000UL-1)

void vbb_IndependentWatchDogInit(void)
{
    IWDG->KR  = 0x5555;                                           // enable write to PR, RLR
    IWDG->PR  = 7;                                        // Init prescaler
    IWDG->RLR = 2047;                                       // Init RLR
    IWDG->KR  = 0xAAAA;                                           // Reload the watchdog
    IWDG->KR  = 0xCCCC;                                           // Start the watchdog
}
